﻿using System;
using System.Collections.Generic;

namespace AntBot.Algorithms
{
	public class Dijkstra
	{
		private readonly Timer mTimer;

		private int[] Dist { get; set; }
		private int[] Path { get; set; }

		private int mLocation;
		private int mNewLocation;
		private ushort mLocationX;
		private ushort mLocationY;
		private ushort mNewLocationX;
		private ushort mNewLocationY;
		private readonly ushort mGridX;
		private readonly ushort mGridY;
		private readonly ushort mGridRealX;
		private readonly ushort mGridRealY;
		private readonly ushort mGridXMinus1;
		private readonly ushort mGridXLog2;
		private readonly int mFullSize;
		private int mNewLocX;
		private int mNewLocY;
		private double mFromRadius;

		public int SearchRadius2 { get; set; }
		private int fromRadius2;
		public int FromRadius2
		{
			get { return fromRadius2; }
			set
			{
				fromRadius2 = value;
				mFromRadius = Math.Sqrt(fromRadius2);
			}
		}

		public byte Goal { get; set; }
		public byte GoalsCount { get; set; }

		private readonly byte[,] mGrid;
		private readonly sbyte[,] mDirection = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };

// ReSharper disable UnusedAutoPropertyAccessor.Local
		public double CompletedTime { get; private set; }
// ReSharper restore UnusedAutoPropertyAccessor.Local

		public List<Location> AchievedGoals { get; private set; }

		public bool IsStopped
		{
			get { return mStopped; }
		}

		private bool mStop;
		private bool mStopped = true;

		private readonly PriorityQueueB<int> queue;

		public void Stop()
		{
			mStop = true;
		}

		private void InitAllStartingPoints(Location fromLoc)
		{
			if (FromRadius2 == 0)
			{
				mLocation = (fromLoc.Y << mGridXLog2) | fromLoc.X;
				Dist[mLocation] = 0;
				Path[mLocation] = -1;
				queue.Push(mLocation);
				return;
			}

			int startX = (int)Math.Floor(fromLoc.X - mFromRadius);
			int endX = (int)Math.Ceiling(fromLoc.X + mFromRadius);

			int startY = (int)Math.Floor(fromLoc.Y - mFromRadius);
			int endY = (int)Math.Ceiling(fromLoc.Y + mFromRadius);

// ReSharper disable TooWideLocalVariableScope
			int adjustedX;
			int adjustedY;
// ReSharper restore TooWideLocalVariableScope

			for (int x = startX; x < endX; x++)
				for (int y = startY; y < endY; y++)
					if (Torus.Distance2(x, y, fromLoc.X, fromLoc.Y, mGridRealX, mGridRealY) <= FromRadius2)
					{
						adjustedX = Torus.Adjust(x, mGridRealX);
						adjustedY = Torus.Adjust(y, mGridRealY);
						if (mGrid[adjustedX, adjustedY] != Router.WATER)
						{
							mLocation = (adjustedY << mGridXLog2) | adjustedX;
							Dist[mLocation] = 0;
							Path[mLocation] = -1;
							queue.Push(mLocation);
						}
					}
			
		}

		public void DoWork(Location fromLoc)
		{
			#if DEBUGTIMER
			mTimer.Start();
			#endif

			mStop = false;
			mStopped = false;
			AchievedGoals = null;
			queue.Clear();

			fromLoc = Torus.Adjust(fromLoc, mGridRealX, mGridRealY);

			// Set distance to all nodes to infinity)
			for (int i = 0; i < mFullSize; i++)
				Dist[i] = int.MaxValue;

			// Set distance to 0 for all starting points and the previous node to null (-1)
			// Push them to queue
			InitAllStartingPoints(fromLoc);

			while (queue.Count > 0 && !mStop)
			{
				mLocation = queue.Pop();
				mLocationX = (ushort)(mLocation & mGridXMinus1);
				mLocationY = (ushort)(mLocation >> mGridXLog2);

				// Find the nodes that connects to and perform relax
				for (int i = 0; i < 4; i++)
				{
					//a.l. Wrapped edges
					mNewLocX = (mLocationX + mDirection[i, 0]);
					mNewLocY = (mLocationY + mDirection[i, 1]);

					if (mNewLocX >= mGridRealX) mNewLocX -= mGridRealX;
					if (mNewLocX < 0) mNewLocX += mGridRealX;

					if (mNewLocY >= mGridRealY) mNewLocY -= mGridRealY;
					if (mNewLocY < 0) mNewLocY += mGridRealY;

					mNewLocationX = (ushort)mNewLocX;
					mNewLocationY = (ushort)mNewLocY;
					mNewLocation = (mNewLocationY << mGridXLog2) | mNewLocationX;

					// Unbreakeable?
					if (mGrid[mNewLocationX, mNewLocationY] == Router.WATER)
						continue;

					if (SearchRadius2 > 0)
						if (Torus.Distance2(
								mNewLocationX, mNewLocationY, fromLoc.X, fromLoc.Y,
								mGridRealX, mGridRealY) > SearchRadius2)
							continue;

					if (Dist[mNewLocation] <= Dist[mLocation] + 1) continue;
					Dist[mNewLocation] = Dist[mLocation] + 1;
					Path[mNewLocation] = mLocation;

					if (GoalsCount > 0)
						if (mGrid[mNewLocationX, mNewLocationY] == Goal)
						{
							if (AchievedGoals == null)
								AchievedGoals = new List<Location>(GoalsCount);

							AchievedGoals.Add(new Location(mNewLocationX, mNewLocationY));

							if (--GoalsCount <= 0)
							{
								mStop = true;
								break;
							}
						}

					queue.Push(mNewLocation);
				}
			}
			mStopped = true;
			#if DEBUGTIMER
			CompletedTime = mTimer.GetTime();
			#endif
		}

		public Dijkstra(byte[,] grid)
		{
			if (grid == null)
				throw new Exception("Grid cannot be null");

			mTimer = new Timer();

			Goal = 0;
			GoalsCount = 0;
			SearchRadius2 = 0;
			FromRadius2 = 0;

			//a.l. Permit to work with any grid sizes (not only pow of 2)
			mGridRealX = (ushort)(grid.GetUpperBound(0) + 1);
			mGridRealY = (ushort)(grid.GetUpperBound(1) + 1);

			mGridX = PathFinderFast.FindClosestPowOf2(mGridRealX);
			mGridY = PathFinderFast.FindClosestPowOf2(mGridRealY);

			mFullSize = mGridX * mGridY;

			if (mGridRealX != mGridX || mGridRealY != mGridY)
			{
				mGrid = new byte[mGridX, mGridY];
				//fill for mGridRealX and mGridRealY. Rest will be 0 (wall)
				for (int x = 0; x < mGridRealX; x++)
					for (int y = 0; y < mGridRealY; y++)
						mGrid[x, y] = grid[x, y];
			}
			else
			{
				mGrid = grid;
			}

			mGridXMinus1 = (ushort)(mGridX - 1);
			mGridXLog2 = (ushort)Math.Log(mGridX, 2);

			Dist = new int[mFullSize];
			Path = new int[mFullSize];

			queue = new PriorityQueueB<int>(new DistComparer(Dist));
		}

		public List<Location> GetReversedPath(Location toLoc)
		{
			mLocation = toLoc.Y*mGridX + toLoc.X;
			if (Dist[mLocation] == int.MaxValue) return null;

			List<Location> path = new List<Location>(10);
			for (; mLocation != -1; mLocation = Path[mLocation])
			{
				mLocationX = (ushort)(mLocation & mGridXMinus1);
				mLocationY = (ushort)(mLocation >> mGridXLog2);
				Location l = new Location(mLocationX, mLocationY);
				path.Add(l);
			}
			return path;
		}

		public List<Location> GetPath(Location toLoc)
		{
			List<Location> path = GetReversedPath(toLoc);
			if (path != null) path.Reverse();
			return path;
		}

		public bool ChangeSquareValue(ushort x, ushort y, byte newValue)
		{
			if (mGrid == null) return false;
			if (x >= mGridRealX) return false;
			if (y >= mGridRealY) return false;
			mGrid[x, y] = newValue;
			return true;
		}

		#region Inner Classes
		internal class DistComparer : IComparer<int>
		{
			readonly int[] mDist;

			public DistComparer(int[] dist)
			{
				mDist = dist;
			}

			#region IComparer Members
			public int Compare(int a, int b)
			{
				if (mDist[a] > mDist[b]) return 1;
				if (mDist[a] < mDist[b]) return -1;
				return 0;
			}
			#endregion
		}
		#endregion
	}
}